#ifndef __PRETREATMENT_MAP__
#define __PRETREATMENT_MAP__

#include <dispatch_system/moying_map.h>
#include <vector>

namespace moying
{
namespace navigation
{



class PretreatmentMap{

public:
    enum CellType{
        kFreeCell,
        kInflateCell,
        kObstacleCell
    };

    //cell排斥信息
    struct MapCellRepel
    {
        MapCell cell;
        float   x_repel;    //障碍物对cell的x方向排斥力
        float   y_repel;    //障碍物对cell的y方向排斥力
    };

    PretreatmentMap(MoyingMap* moying_map,float robot_length=1,float robot_width=0.618,float l_padding=0.2,float w_padding=0.2);
    bool correctMapCellCoordinate(CorrectCells& cci_vec,int x_cell_size,int y_cell_size);
    void setRobotSize(double robot_length,double robot_width,double l_padding,double w_padding);
    bool isPointInRect(Coordinate pt,Coordinate lt,Coordinate rt,Coordinate rb,Coordinate lb);
    double robot_length(){return robot_length_;}
    double robot_width(){return robot_width_;}

private:

    /**
     * @brief 和邻近的cell对齐导航点
     * @param   [IN] cci_vec    更改导航点的cell数组
     * @param   x_cell_size     x方向的cell数量 
     * @param   y_cell_size     y方向的cell数量
     */ 
    void alignCellNavPos(CorrectCells& cci_vec,int x_cell_size,int y_cell_size);

    /**
     * @brief 获取中心cell周围8个cell坐标,返回的cell以从左到右,从下到上的顺序存储
     * @param   [IN] center_cell 中心cell
     * @param   [OUT] surrounding_cells 周围8个cell
     * @return  成功或失败
     */ 
    bool getSurroundingMapCells(MapCell center_cell,std::vector<MapCellRepel>& surrounding_cells);

    /**
     * @brief 获取cell在原始地图中占用的坐标
     * @param   [IN] cell 地图cell
     * @param   [OUT] coors 原始地图坐标
     * @return  成功或失败
     */ 
    bool getRawMapCoorInCell(MapCell cell,std::vector<CoordinateInt>& coors);

    /**
     * @brief 计算目标坐标点离cell中障碍物的最近距离
     * @param   [IN] cell 地图cell
     * @param   [IN] coor 目标坐标点
     * @param   [OUT] min_dis 最短距离
     * @return  true:成功, false:cell中没有障碍物
     */ 
    bool calculateCellObstacleMinDis(MapCell cell,CoordinateInt coor,float& min_dis);

    /**
     * @brief 判断mapcell类型
     * @param   [IN] cell 地图cell
     * @return  cell类型
     */ 
    CellType getMapCellType(MapCell cell);

    /**
     * @brief 检测机器人在指定姿态下是否会碰撞,会和局部障碍物以及静态地图做碰撞检测
     * @param   [IN] x 机器人在原始代价地图中的x坐标
     * @param   [IN] y 机器人在原始代价地图中的y坐标
     * @param   [IN] th 机器人朝向,0-7表示逆时针的8个方向
     * @return  true:碰撞 false:不会碰撞
     */ 
    bool checkCollision(int x,int y,int th);

    bool cost_calculated(){return cost_calculated_;}

private:
    MoyingMap* moying_map_ptr_=nullptr;
    std::vector<std::vector<CoordinateInt>> raw_coor_in_cell_;   //每一个cell中包含的原始地图坐标点
    std::vector<std::pair<bool,CellType>> cell_type_;   //每个cell的类型 , pair第一个值表示该cell类型是否计算过,当第一个值为true时,第二值为cell的类型
    int scale_;
    double robot_length_;
    double robot_width_;
    double l_padding_;
    double w_padding_;
    CorrectCells ccis_;
    bool cost_calculated_ = false;  //是否计算过cell的cost

};


}
}

#endif